/*
 * @Description: 多平面裁剪器，多个平面参数裁剪，自然包括了单平面裁剪器，通过参数可以设置
 * @Author: Sang Hao
 * @Date: 2021-11-15 15:08:39
 * @LastEditTime: 2021-11-22 21:24:53
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_MODELS_CLOUD_FILTER_MULTI_PLANE_CLIPPER_FILTER_HPP_
#define LIDAR_SLAM_MODELS_CLOUD_FILTER_MULTI_PLANE_CLIPPER_FILTER_HPP_

#include <vector>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>
#include <pcl/ModelCoefficients.h>
#include "lidar_slam/models/cloud_filter/cloud_filter_interface.hpp"
#include "lidar_slam/models/extract/plane_ransac_extract.hpp"

namespace lidar_slam {
class MultiPlaneClipperFilter : public CloudFilterInterface {
private:
	/* 保存平面参数向量 */
	// std::vector<pcl::ModelCoefficients> vector_model_coeff_;
	/* 平面提取类，用来得到多个平面，TODO */
	std::shared_ptr<PlaneRansacExtract> horizontal_plane_extract_;
	std::shared_ptr<PlaneRansacExtract> vertical_plane_extract_;
	/* 参数 */
	float plane_clipper_tolerance_;

private:

	Eigen::Vector4f GetPlaneParamFromCoeffAndToler(pcl::ModelCoefficients& plane_model_coeff,
		float plane_clipper_tolerance);

public:
	MultiPlaneClipperFilter(const YAML::Node& config_node);
	/* 得到平面参数的函数也设为公有，因为这平面参数可以作为很好的观测用于对移动平台的位姿估计 */
	// bool GetVectorModelCoeff(const CloudData::CLOUD_PTR& input_cloud_ptr); 
	bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudData::CLOUD_PTR& filter_cloud_ptr) override;

};
}

#endif